#pragma once
#ifndef COLLLISION_DETECTOR_H
#define COLLLISION_DETECTOR_H

#include "basic_typedef.h"
#include <CGAL/squared_distance_2.h>
#include <CGAL/minkowski_sum_2.h>

class CollisionDetector
{
public:
	
	////////////////////////
	// CTORS
	////////////////////////
	/*	Parameters:
			radius	- radius of robot
			Room	- describes the boundary of the scenario
	*/
	CollisionDetector(vector<Polygon_2>	robot_polygons, Obstacles *obs): 
				m_robot_polygons(robot_polygons), m_obs(obs) 
	{
		// Construction of the polygon set, used for collision detection
		if (!m_obs->empty())
		{
			for (Obstacles::iterator iter = m_obs->begin(); iter != m_obs->end(); iter++)
			{
				// For every obstacle calculate its Minkowski sum with the "robot"
				Polygon_with_holes_2	poly_wh = minkowski_sum_2(*iter, negative_polygon(m_robot_polygons[0]));
				Polygon_with_holes_2	poly_wh2 = minkowski_sum_2(*iter, negative_polygon(m_robot_polygons[1]));
				
				// Add the result to the polygon set
				m_robot_1_poly_set.join(poly_wh);
				m_robot_2_poly_set.join(poly_wh2);
			}
		}
	}
	
	~CollisionDetector(){};

	////////////////////////
	// Queries
	////////////////////////
	
	Polygon_2 negative_polygon(Polygon_2 polygon)
	{
		Polygon_2 negative_robot;
		for (Polygon_2::Vertex_iterator verteces_it = polygon.vertices_begin();
			verteces_it != polygon.vertices_end();
			++verteces_it) {
			Point_2 nvx = CGAL::ORIGIN - Vector_2(CGAL::ORIGIN, (*verteces_it));
			negative_robot.push_back(CGAL::ORIGIN - Vector_2(CGAL::ORIGIN, (*verteces_it)));
		}

		return negative_robot;
	}

	/*	Check if the given configuration is collision free (returns true for free samples) */
	bool valid_conf(Conf robot_1_conf, Conf robot_2_conf)
	{
		Polygon_2 new_robot_1 = get_new_polygon(robot_1_conf, m_robot_polygons[0]);
		Polygon_2 new_robot_2 = get_new_polygon(robot_2_conf, m_robot_polygons[1]);

		return (((m_robot_1_poly_set.oriented_side(robot_1_conf) == CGAL::ON_NEGATIVE_SIDE)) &&
				((m_robot_2_poly_set.oriented_side(robot_2_conf) == CGAL::ON_NEGATIVE_SIDE)) &&
				(!CGAL::do_intersect(new_robot_1,new_robot_2)));
	}

	/*	Validate the connection between two configurations by 
		sampling along a line.
		Input:	* Start and target configurations
				* Eps value that determines the size of the steps 
					that are taken along the line
	*/

	bool	local_planner(ConfSet start, ConfSet target, double step_size)
	{
		Point_2 robot_1_start = start[0];
		Point_2 robot_2_start = start[1];
		Point_2 robot_1_target = target[0];
		Point_2 robot_2_target = target[1];

		double robot_1_x1 = CGAL::to_double(robot_1_start.x());
		double robot_1_y1 = CGAL::to_double(robot_1_start.y());
		double robot_1_x2 = CGAL::to_double(robot_1_target.x());
		double robot_1_y2 = CGAL::to_double(robot_1_target.y());

		double robot_1_distance = sqrt(pow(robot_1_x1 - robot_1_x2, 2) + pow(robot_1_y1 - robot_1_y2,2));

		// calculate how many steps are required
		int robot_1_step_num = floor((robot_1_distance - step_size) / step_size);
		double robot_1_vx = robot_1_x2 - robot_1_x1;
		double robot_1_vy = robot_1_y2 - robot_1_y1;

		double robot_2_x1 = CGAL::to_double(robot_2_start.x());
		double robot_2_y1 = CGAL::to_double(robot_2_start.y());
		double robot_2_x2 = CGAL::to_double(robot_2_target.x());
		double robot_2_y2 = CGAL::to_double(robot_2_target.y());

		double robot_2_distance = sqrt(pow(robot_2_x1 - robot_2_x2, 2) + pow(robot_2_y1 - robot_2_y2,2));

		// calculate how many steps are required
		int robot_2_step_num = floor((robot_2_distance - step_size) / step_size);
		double robot_2_vx = robot_2_x2 - robot_2_x1;
		double robot_2_vy = robot_2_y2 - robot_2_y1;

		int stemp_num = (robot_2_step_num < robot_1_step_num) ? robot_1_step_num : robot_2_step_num;

		double robot_1_partial_offset = step_size / (robot_1_distance - step_size);
		double robot_2_partial_offset = step_size / (robot_2_distance - step_size);

		for (int i = 1; i <= stemp_num; i++)
		{
			// generate a configuration for every step
			double robot_1_offset =  i * robot_1_partial_offset;
			double robot_1_currx = robot_1_x1 + robot_1_vx * robot_1_offset;
			double robot_1_curry = robot_1_y1 + robot_1_vy * robot_1_offset;

			Conf robot_1_current_conf(robot_1_currx, robot_1_curry);

			// generate a configuration for every step
			double robot_2_offset =  i * robot_2_partial_offset;
			double robot_2_currx = robot_2_x1 + robot_2_vx * robot_2_offset;
			double robot_2_curry = robot_2_y1 + robot_2_vy * robot_2_offset;

			Conf robot_2_current_conf(robot_2_currx, robot_2_curry);

			// If an intermidiate configuration is invalid, return false
			if (!valid_conf(robot_1_current_conf, robot_2_current_conf))
			{
				return false;
			}
		}

		// GREAT SUCCESS!
		return true;
	}

	Polygon_2 get_new_polygon( Conf new_robot_point, Polygon_2 original_robot ) 
	{
		Polygon_2 new_polygon;
		for (Polygon_2::Vertex_iterator v_it = original_robot.vertices_begin();
			 v_it != original_robot.vertices_end();
			 ++v_it)
		{
			Point_2 new_point(v_it->x() + new_robot_point.x(),v_it->y() + new_robot_point.y());
			new_polygon.push_back(new_point);
		}

		return new_polygon;
	}


	////////////////////////
	// Data Members
	////////////////////////
	vector<Polygon_2>			m_robot_polygons;
	Obstacles					*m_obs;		// Collection of polygonal obstacles
	Polygon_set_2				m_robot_1_poly_set; // Polygon set, for collision detection
	Polygon_set_2				m_robot_2_poly_set; // Polygon set, for collision detection
};

#endif